#include <stdio.h>
#include <stdbool.h>
#define TOTAL_SECTOR       200     // 一圈步数
#define ENCODER_MAX        16384   // 编码器最大值
#define IN_RAM(x)          (x)
// 复现bug的测试用例，特点为最后一个元素小于16384
int16_t calibration[201]={
    51,134,215,296,379,461,543,624,707,789,871,953,1036,1118,1200,1281,1364,1446,1527,1609,
    1692,1774,1855,1936,2018,2100,2182,2263,2345,2427,2509,2590,2672,2754,2836,2917,2999,3081,3164,
    3245,3327,3408,3491,3572,3653,3735,3817,3898,3980,4061,4143,4224,4306,4387,4469,4551,4632,4714,
    4796,4878,4960,5042,5124,5206,5288,5370,5452,5534,5616,5697,5780,5861,5943,6024,6106,6188,6269,
    6350,6433,6514,6596,6677,6760,6842,6923,7005,7087,7170,7251,7333,7415,7497,7579,7660,7742,7824,
    7905,7987,8069,8147,8229,8310,8393,8474,8557,8638,8720,8803,8885,8966,9049,9131,9214,9295,9378,
    9460,9542,9624,9706,9788,9870,9951,10033,10115,10197,10278,10360,10442,10524,10605,10687,10768,
    10851,10932,11015,11096,11179,11260,11342,11424,11507,11588,11670,11752,11834,11915,11997,12079,
    12161,12243,12324,12407,12489,12570,12651,12734,12817,12898,12980,13062,13145,13228,13309,13391,
    13475,13557,13638,13721,13804,13885,13967,14048,14132,14213,14294,14376,14458,14539,14620,14702,
    14785,14867,14949,15031,15114,15196,15279,15361,15444,15527,15609,15691,15774,15857,15939,16021,
    16105,16187,16269,16351
};
// 正常的测试用例
// int16_t calibration[201]={
//     247,   326,   407,   489,   572,   654,   735,   818,   901,   983,  1064,  1148,  1230,  1312,  1394,  1477,  1559,  1641,  1722,  1805,
//     1887,  1968,  2049,  2132,  2215,  2296,  2377,  2459,  2542,  2623,  2704,  2788,  2871,  2952,  3034,  3117,  3199,  3281,  3363,  3445,
//     3528,  3609,  3690,  3773,  3855,  3935,  4016,  4099,  4181,  4262,  4343,  4425,  4507,  4588,  4670,  4752,  4835,  4916,  4998,  5081,
//     5163,  5245,  5327,  5409,  5491,  5573,  5655,  5737,  5819,  5900,  5981,  6063,  6145,  6226,  6307,  6389,  6471,  6552,  6634,  6716,
//     6798,  6880,  6962,  7044,  7127,  7208,  7290,  7372,  7455,  7537,  7618,  7701,  7783,  7865,  7946,  8028,  8109,  8191,  8273,  8355,
//     8436,  8518,  8599,  8682,  8763,  8846,  8928,  9010,  9093,  9175,  9257,  9340,  9422,  9505,  9587,  9669,  9751,  9833,  9915,  9997,
//     10079, 10161, 10242, 10324, 10407, 10488, 10570, 10651, 10734, 10816, 10898, 10980, 11063, 11145, 11227, 11308, 11392, 11473, 11555, 11637,
//     11720, 11801, 11883, 11964, 12046, 12127, 12208, 12290, 12373, 12454, 12535, 12616, 12698, 12780, 12862, 12943, 13026, 13108, 13189, 13272,
//     13355, 13437, 13518, 13600, 13683, 13765, 13846, 13928, 14011, 14092, 14173, 14255, 14337, 14418, 14499, 14581, 14663, 14745, 14826, 14909,
//     14991, 15073, 15155, 15237, 15320, 15402, 15484, 15566, 15650, 15731, 15813, 15896, 15979, 16060, 16141, 16223, 16306, 16387, 16469, 16552,
// };

// input的取值范围是0 ~ ENCODER_MAX-1
static void IN_RAM(calibration_angle)(int input, int *electro, int *machanic)
{
    int sector = 0;
    int angle;
    int16_t a, b;
    // 处理跳变区
    if (input < calibration[0]) {
        input += ENCODER_MAX;
    }
    // 二分法查找扇区
    int left = 0;
    int right = TOTAL_SECTOR; // 数组大小是 TOTAL_SECTOR + 1
    while (left <= right) {
        int mid = left + (right - left) / 2;
        if (calibration[mid] <= input) {
            sector = mid;
            left = mid + 1;
        } else {
            right = mid - 1;
        }
    }
    // 确定a和b的值，不需要处理最后一个扇区的情况
    // 已经提前处理了 calibration[200] = calibration[0] + ENCODER_MAX; 
    a = calibration[sector];
    b = calibration[sector + 1];
    
    // 计算角度
    angle = 256 * (input - a) / (b - a);
    angle += 256 * (sector % 4);
    // 根据返回类型调整结果, 确保在0-1023范围内
    *electro = angle % 1024;
    // angle % 1024 + 1024 * (sector / 4) 范围0-51199
    // (angle % 1024 + 1024 * (sector / 4)) * 8 / 25 范围0-16383
    *machanic = (angle % 1024 + 1024 * (sector / 4)) * 8 / 25; 
}
int main()
{
    calibration[200] = calibration[0] + ENCODER_MAX; 

    for(int i=16000; i<16384; i++){
        int electro;
        int machanic;
        int input = i;
        calibration_angle(input, &electro, &machanic);
        printf("%d->%d\n",input,electro);
    }
    for(int i=0; i<500; i++){
        int electro;
        int machanic;
        int input = i;
        calibration_angle(input, &electro, &machanic);
        printf("%d->%d\n",input,electro);
    }
    return 0;
}
/*
16346->752
16347->755
16348->758
16349->761
16350->764
16351->768
16352->-256 # 期望输出范围0-1023，实际得到负数
16353->-252
16354->-249
16355->-246
16356->-243
16357->-240
16358->-237
16359->-234
16360->-231
16361->-228
16362->-225
16363->-222
16364->-218
16365->-215
16366->-212
16367->-209
16368->-206
16369->-203
16370->-200
16371->-197
16372->-194
16373->-191
16374->-188
16375->-185
16376->-181
16377->-178
16378->-175
16379->-172
16380->-169
16381->-166
16382->-163
16383->-160
0->868
1->871
2->874
3->877
4->880
5->883
6->886
7->889
8->892
*/